#include "KeyPointSet.h"
#include <algorithm>
#include <vector>

KeyPointSet::KeyPointSet(){
	stabilety_threshold 	= 0;
	valid_key_points 		= 0;
	invalid_key_points 		= 0;
	nr_valid_points 		= 0;
	nr_invalid_points 		= 0;  
};
KeyPointSet::~KeyPointSet(){
	if(valid_key_points != 0)
	{
		for(int i = 0; i < nr_valid_points; i++)
		{
			if(valid_key_points[i] == 0){delete valid_key_points[i];}
		}
		delete valid_key_points;
		nr_valid_points = 0;
	}
	if(invalid_key_points != 0)
	{
		for(int i = 0; i < nr_invalid_points; i++)
		{
			if(invalid_key_points[i] == 0){delete invalid_key_points[i];}
		}
		delete invalid_key_points;
		nr_invalid_points = 0;
	}
};

KeyPointSet * KeyPointSet::clone(){
	printf("clone KeyPointSet\n");
	KeyPointSet * keypointset = new KeyPointSet();
	return keypointset;
};

void KeyPointSet::print(){
		ROS_INFO("stabilety_threshold:	%f",stabilety_threshold);
		ROS_INFO("nr_valid_points:		%i",nr_valid_points);
		ROS_INFO("nr_invalid_points:		%i",nr_invalid_points);
		for(int i = 0; i < nr_valid_points;i++)		{ROS_INFO("\n\nValid[%i]",i);	valid_key_points[i]->print();}
		for(int i = 0; i < nr_invalid_points;i++)	{ROS_INFO("\n\nInvalid[%i]",i);	invalid_key_points[i]->print();}
};

bool comparison(KeyPoint * a, KeyPoint * b) { return (a->stabilety > b->stabilety); }

void KeyPointSet::sortKeyPoints(){
	using namespace std;
	ROS_INFO("Line:%i",__LINE__);
	vector< KeyPoint* > myvector (valid_key_points, valid_key_points+nr_valid_points);
	sort (myvector.begin(), myvector.end(), comparison);
	ROS_INFO("Line:%i Nr invalid: %i",__LINE__,nr_invalid_points);
	vector< KeyPoint* > myvector2 (invalid_key_points, invalid_key_points+nr_invalid_points);
	sort (myvector2.begin(), myvector2.end(), comparison);
	ROS_INFO("%i",__LINE__);
	int counter = 0;
	vector< KeyPoint* >::iterator it;
	ROS_INFO("%i",__LINE__);
	for (it=myvector.begin(); it!=myvector.end(); ++it){
		valid_key_points[counter] = *it;
		counter++;
	}
	ROS_INFO("%i",__LINE__);
	counter = 0;
	for (it=myvector2.begin(); it!=myvector2.end(); ++it){
		invalid_key_points[counter] = *it;
		counter++;
		
	}
	ROS_INFO("%i",__LINE__);
	if(nr_invalid_points > 0  && nr_valid_points > 0)
	{	
		if(invalid_key_points[nr_invalid_points-1]->stabilety < valid_key_points[nr_valid_points-1]->stabilety){
			stabilety_threshold = invalid_key_points[nr_invalid_points-1]->stabilety;
		}else{
			stabilety_threshold = valid_key_points[nr_valid_points-1]->stabilety;
		}
	}else if(nr_invalid_points > 0){
		stabilety_threshold = invalid_key_points[nr_invalid_points-1]->stabilety;
	}else if(nr_valid_points > 0){
		stabilety_threshold = valid_key_points[nr_valid_points-1]->stabilety;
	}else{
		stabilety_threshold = -1;
	}
	ROS_INFO("%i",__LINE__);

	 
}
